#include <ros/ros.h>
#include <geometry_msgs/Twist.h>//用于消息发布 消息的类型
#include "nav_msgs/Path.h"//路径的消息类型
#include <geometry_msgs/PoseStamped.h>//路径点的消息类型
#include "tf2_ros/transform_listener.h"//map-base_link的tf变幻 用于机器人定位
#include "tf2_ros/buffer.h"
#include "tf2/utils.h"//用于四元数转化为欧拉角

#include "example/my_msg.h"//你自己定义的消息
int main(int argc,char **argv)
{
    ros::init(argc,argv,"example2");
    ros::NodeHandle nh;
    //-----------------消息发布----------------------------------//
    ros::Publisher pub_cmd= nh.advertise<geometry_msgs::Twist>("cmd_vel",10);
    //                                    消息格式(速度类型的)     消息的名称 消息的大小
    ros::Publisher pub_globalPlan= nh.advertise<nav_msgs::Path>("globalPlan",10,true);//类型为true保证后来的订阅（rviz）也能得到最近的一个数据
    ros::Publisher pub_my_data= nh.advertise<example::my_msg>("my_data",10);

    nav_msgs::Path path;
    geometry_msgs::PoseStamped path_point;
    path.header.frame_id="map";//指定路径坐标系  地图坐标系
    // path.header.frame_id="base_link";//机器人坐标系
    for(size_t i=0;i<20;i++)//将路径点添加到路径中
    {
        path_point.pose.position.x=-2+i*0.1;
        path_point.pose.position.y=-0.5;
        path.poses.push_back(path_point);
    }
    pub_globalPlan.publish(path);

    //---------------自定义消息----------------------------
    example::my_msg my_data;

    ros::Rate rate(10);//设置ros循环周期为10HZ,即他的休眠频率为10HZ
    while(ros::ok())
    {
        tf2_ros::Buffer tfBuffer;
        tf2_ros::TransformListener tfListener(tfBuffer);//创建监听者
        geometry_msgs::TransformStamped transformS;
        geometry_msgs::TransformStamped transformS2;//第二个坐标变化

        try{
            //这里有第一对坐标关系变换 map坐标变化到车体中心坐标
            std::string base_frame="map";
            std::string child_frame="base_link";
            transformS=tfBuffer.lookupTransform(base_frame,child_frame,ros::Time(0),ros::Duration(1.0));

            // child_frame="camera";
            // transformS2=tfBuffer.lookupTransform(base_frame,child_frame,ros::Time(0),ros::Duration(1.0));

            // double yaw=tf2::getYaw(transformS2.transform.rotation);
            
            my_data.x=transformS.transform.translation.x;
            // my_data.yaw=yaw;
            pub_my_data.publish(my_data);
            //if(transformS.transform.translation.x<path.poses.back().pose.position.x)
            if(transformS.transform.translation.x<0)
            {
                geometry_msgs::Twist cmd_vel;
                cmd_vel.linear.x=0.2;//存储消息数据
                pub_cmd.publish(cmd_vel);//发布消息
            }
            // else if(yaw>0)
            // {
            //     geometry_msgs::Twist cmd_vel;
            //     cmd_vel.angular.z=-0.2;//存储消息数据
            //     pub_cmd.publish(cmd_vel);//发布消息
            // }
            else{
                geometry_msgs::Twist cmd_vel;
                cmd_vel.linear.x=0;//存储消息数据
                pub_cmd.publish(cmd_vel);//发布消息
            }

        }
        catch(tf2::TransformException &ex){
            ROS_WARN("NO TF!");
        }
        // geometry_msgs::Twist cmd_vel;
        // cmd_vel.linear.x=10;//存储消息数据
        // pub_cmd.publish(cmd_vel);//发布消息

        rate.sleep();//之前设定周期为10HZ，该休眠函数会让一个循环的执行补足到0.1s后才执行第二次


        //回调函数的执行需要相应的语句
        //如果在循环体中处理
        // ros::spinOnce();

        //如果在无循环状态下进行处理
        //ros::spin();//该语句会一直等待执行回调函数，delay_portmax
    }
    
    return 0;
}
